#include <iostream>
#include "walk.h"
#include <cmath>

Walk::Walk()
{
    big_len = 290;    // 大腿长度
    small_len = 275;  // 小腿长度
    foot_height = 140.4;  // 脚高度
    delta_z = 0;
    delta_x = 0;
    delta_x_count = 0;
    hip_pitch_bias = 0;
    status = "standing";
    left_leg_position[0] = 0;
    left_leg_position[1] = 0;
    left_leg_position[2] = big_len+small_len+foot_height;
    right_leg_position[0] = 0;
    right_leg_position[1] = 0;
    right_leg_position[2] = big_len+small_len+foot_height;
}

void Walk::Init(float *joints, int height , bool back)
{
    if(delta_z <= height && status == "standing")
    {
        delta_z += 0.1;
        left_leg_position[2] = big_len+small_len+foot_height-delta_z;
        right_leg_position[2] = big_len+small_len+foot_height-delta_z;
        Inverse(left_leg_position, right_leg_position, joints, back);
    }
    else
    {
        status = "ready";
    }
}

bool Walk::Back(float *joints)
{
    static float local_count = 0;
    local_count += 0.05;
    if(local_count <= 50)
    {
        hip_pitch_bias = 0.34 * sin(M_PI/2*local_count/50);
        Inverse(left_leg_position, right_leg_position, joints, true);
        return false;
    }
    else
    {
        return true;
    }
}

bool Walk::Inverse(float *left_position, float *right_position, float *joints, bool back)
{
    // 解算左脚
    float distance = 0;
    distance = sqrt(left_position[0]*left_position[0]+left_position[1]*left_position[1]+(left_position[2]-foot_height)*(left_position[2]-foot_height));
    joints[1] = atan2(left_position[0], left_position[2] - foot_height);
    if(joints[1] > 0.6112 || joints[1] < -0.6112)
    {
        std::cout << "LHipRoll is out of range" << std::endl;
        std::cout << left_position[0] << ", " << left_position[1] << ", " << left_position[2] << std::endl;
        return false;
    }
    joints[5] = -joints[1];
    joints[3] = M_PI - acos((big_len*big_len+small_len*small_len-distance*distance)/(2*big_len*small_len));
    if(joints[3] > 2.2672 || joints[3] < -0.262)
    {
        std::cout << "LKnee is out of range" << std::endl;
        std::cout << left_position[0] << ", " << left_position[1] << ", " << left_position[2] << std::endl;
        return false;
    }
    float theta_1 = atan2(left_position[1], sqrt(left_position[0]*left_position[0]+(left_position[2]-foot_height)*(left_position[2]-foot_height)));
    joints[2] = -acos((big_len*big_len+distance*distance-small_len*small_len)/(2*big_len*distance)) - theta_1;
    if(back == true)
    {
        joints[2] += hip_pitch_bias;
    }
    if(joints[2] > 0.6112 || joints[2] < -1.4872)
    {
        std::cout << "LHipPitch is out od range" << std::endl;
        std::cout << left_position[0] << ", " << left_position[1] << ", " << left_position[2] << std::endl;
        return false;
    }
    joints[4] = theta_1 - acos((small_len*small_len+distance*distance-big_len*big_len)/(2*small_len*distance));
    if(joints[4] > 0.6112 || joints[4] < -1.4872)
    {
        std::cout << "LAnklePitch is out of range" << std::endl;
        std::cout << left_position[0] << ", " << left_position[1] << ", " << left_position[2] << std::endl;
        return false;
    }

    // 解算右脚
    distance = sqrt(right_position[0]*right_position[0]+right_position[1]*right_position[1]+(right_position[2]-foot_height)*(right_position[2]-foot_height));
    joints[7] = atan2(right_position[0], right_position[2] - foot_height);
    if(joints[7] > 0.6112 || joints[7] < -0.6112)
    {
        std::cout << "RHipRoll is out of range" << std::endl;
        std::cout << right_position[0] << ", " << right_position[1] << ", " << right_position[2] << std::endl;
        return false;
    }
    joints[11] = -joints[7];
    joints[9] = M_PI - acos((big_len*big_len+small_len*small_len-distance*distance)/(2*big_len*small_len));
    if(joints[9] > 2.2672 || joints[9] < -0.262)
    {
        std::cout << "RKnee is out of range" << std::endl;
        std::cout << right_position[0] << ", " << right_position[1] << ", " << right_position[2] << std::endl;
        return false;
    }
    theta_1 = atan2(right_position[1], sqrt(right_position[0]*right_position[0]+(right_position[2]-foot_height)*(right_position[2]-foot_height)));
    joints[8] = -acos((big_len*big_len+distance*distance-small_len*small_len)/(2*big_len*distance)) - theta_1;
    if(back == true)
    {
        joints[8] += hip_pitch_bias;
    }
    if(joints[8] > 0.6112 || joints[8] < -1.4872)
    {
        std::cout << "RHipPitch is out od range" << std::endl;
        std::cout << right_position[0] << ", " << right_position[1] << ", " << right_position[2] << std::endl;
        return false;
    }
    joints[10] = theta_1 - acos((small_len*small_len+distance*distance-big_len*big_len)/(2*small_len*distance));
    if(joints[10] > 0.6112 || joints[10] < -1.4872)
    {
        std::cout << "RAnklePitch is out od range" << std::endl;
        std::cout << right_position[0] << ", " << right_position[1] << ", " << right_position[2] << std::endl;
        return false;
    }
    return true;
}

std::string Walk::get_status()
{
    return status;
}

bool Walk::walk(float *joints)
{
    if(status == "stop" || status == "standing")
    {
        return false;
    }
    delta_x_count += 0.001;

    float amplitude = 45;
    float step_len = 280;
    float step_height = 100;
    if(status == "ready")
    {
        step_len = 310;
        // 起步阶段
        // 第一小节腿不抬起，重心移至右脚
        if(delta_x_count <= 0.33)
        {
            left_leg_position[0] = amplitude*(1-cos(M_PI/2*(delta_x_count/0.33)));
            right_leg_position[0] = amplitude*(1-cos(M_PI/2*(delta_x_count/0.33)));
        }
        // 第二小节左腿往前迈，腿迈快一点
        else if(delta_x_count <= 0.65)
        {
            left_leg_position[0] = amplitude - amplitude*(1-cos(M_PI/2*((delta_x_count-0.33)/0.32)));
            right_leg_position[0] = amplitude - amplitude*(1-cos(M_PI/2*((delta_x_count-0.33)/0.32)));
            right_leg_position[1] = -step_len/2 * (delta_x_count-0.33)/0.32;
            float stairs_height;
            if(delta_x_count <= 0.53)
            {
                stairs_height = step_height*sin(M_PI/2*(delta_x_count-0.33)/0.2);
            }
            else
            {
                stairs_height = step_height;
            }
            left_leg_position[2] = big_len+small_len+foot_height-delta_z-40*sin(M_PI*((delta_x_count-0.33)/0.32))-stairs_height;
            left_leg_position[1] = step_len/2 * (delta_x_count-0.33)/0.32;
        }
        // 第三小节重心移动至左脚
        else if(delta_x_count <= 1.25)
        {
            left_leg_position[0] = -amplitude*sin(M_PI/2*((delta_x_count-0.65)/0.3));
            right_leg_position[0] = -amplitude*sin(M_PI/2*((delta_x_count-0.65)/0.3));
            left_leg_position[1] = step_len/2 - (150+155) * (delta_x_count-0.65)/0.6;
            right_leg_position[1] = -step_len/2 + (150+155)* (delta_x_count-0.65)/0.6;
            float stairs_height;
            if(delta_x_count <= 0.81)
            {
                stairs_height = step_height*sin(M_PI/2*(delta_x_count-0.65)/0.16);
            }
            else
            {
                stairs_height = step_height;
            }
            left_leg_position[2] = big_len+small_len+foot_height-delta_z - step_height + step_height*sin(M_PI/2*(delta_x_count-0.65)/0.6);;
            right_leg_position[2] = big_len+small_len+foot_height-delta_z-80*sin(M_PI*((delta_x_count-0.65)/0.6))-stairs_height;
        }
        if(delta_x_count >= 1.25)
        {
            status = "dynamic";
            delta_x_count = 0;
        }
        Inverse(left_leg_position, right_leg_position, joints, true);
    }
    else
    {
        amplitude = 50;
        step_len = 280;
        if(delta_x_count <= 0.54)
        {
            step_len = 310;
            left_leg_position[0] = amplitude*sin(M_PI/2*delta_x_count/0.27);
            right_leg_position[0] = amplitude*sin(M_PI/2*delta_x_count/0.27);
            left_leg_position[1] = -step_len/2 + step_len * delta_x_count/0.54;
            right_leg_position[1] = step_len/2 - step_len * delta_x_count/0.54;
            float stairs_height;
            if(delta_x_count <= 0.2)
            {
                stairs_height = step_height*sin(M_PI/2*delta_x_count/0.2);
            }
            else
            {
                stairs_height = step_height;
            }
            right_leg_position[2] = big_len+small_len+foot_height-delta_z - step_height + step_height*sin(M_PI/2*delta_x_count/0.54);
            left_leg_position[2] = big_len+small_len+foot_height-delta_z-85*sin(M_PI*delta_x_count/0.54)-stairs_height;
        }
        else if(delta_x_count <= 1.08)
        {
            step_len = 290;
            left_leg_position[0] = -amplitude*sin(M_PI/2*(delta_x_count-0.54)/0.27);
            right_leg_position[0] = -amplitude*sin(M_PI/2*(delta_x_count-0.54)/0.27);
            right_leg_position[1] = -step_len/2 + step_len * (1-cos(M_PI/2*(delta_x_count - 0.54)/0.54));
            left_leg_position[1] = step_len/2 - step_len * (delta_x_count - 0.54)/0.54;
            float stairs_height;
            if(delta_x_count <= 0.94)
            {
                stairs_height = step_height*sin(M_PI/2*(delta_x_count-0.54)/0.4);
            }
            else
            {
                stairs_height = step_height;
            }
            left_leg_position[2] = big_len+small_len+foot_height-delta_z - step_height + step_height*sin(M_PI/2*(delta_x_count-0.54)/0.54);
            right_leg_position[2] = big_len+small_len+foot_height-delta_z-80*sin(M_PI*(delta_x_count-0.54)/0.54)-stairs_height;
        }
        else if(delta_x_count <= 1.62)
        {
            step_len = 300;
            left_leg_position[0] = amplitude*sin(M_PI/2*(delta_x_count-1.08)/0.27);
            right_leg_position[0] = amplitude*sin(M_PI/2*(delta_x_count-1.08)/0.27);
            left_leg_position[1] = -step_len/2 + step_len * (delta_x_count-1.08)/0.54;
            right_leg_position[1] = step_len/2 - step_len * (delta_x_count-1.08)/0.54;
            float stairs_height;
            if(delta_x_count <= 1.38)
            {
                stairs_height = step_height*sin(M_PI/2*(delta_x_count-1.08)/0.3);
            }
            else
            {
                stairs_height = step_height;
            }
            right_leg_position[2] = big_len+small_len+foot_height-delta_z - step_height + step_height*sin(M_PI/2*(delta_x_count-1.08)/0.54);
            left_leg_position[2] = big_len+small_len+foot_height-delta_z-90*sin(M_PI*(delta_x_count-1.08)/0.54)-stairs_height;
        }
        else if(delta_x_count <= 2.16)
        {
            step_len = 300;
            left_leg_position[0] = -amplitude*sin(M_PI/2*(delta_x_count-1.62)/0.27);
            right_leg_position[0] = -amplitude*sin(M_PI/2*(delta_x_count-1.62)/0.27);
            right_leg_position[1] = -step_len/2 + step_len * (1-cos(M_PI/2*(delta_x_count-1.62)/0.54));
            left_leg_position[1] = step_len/2 - step_len * (delta_x_count-1.62)/0.54;
            float stairs_height;
            if(delta_x_count <= 1.82)
            {
                stairs_height = step_height*sin(M_PI/2*(delta_x_count-1.62)/0.2);
            }
            else
            {
                stairs_height = step_height;
            }
            left_leg_position[2] = big_len+small_len+foot_height-delta_z - step_height + step_height*sin(M_PI/2*(delta_x_count-1.62)/0.54);
            right_leg_position[2] = big_len+small_len+foot_height-delta_z-90*sin(M_PI*(delta_x_count-1.62)/0.54)-stairs_height;
        }
        else if(delta_x_count <= 2.66)
        {
            step_len = 300;
            left_leg_position[0] = amplitude*sin(M_PI/2*(delta_x_count-2.16)/0.25);
            right_leg_position[0] = amplitude*sin(M_PI/2*(delta_x_count-2.16)/0.25);
            right_leg_position[1] = step_len/2 - step_len/2 * (delta_x_count-2.16)/0.5;
            left_leg_position[1] = -step_len/2 + step_len/2 * (delta_x_count-2.16)/0.5;
            float stairs_height;
            if(delta_x_count <= 2.36)
            {
                stairs_height = step_height*sin(M_PI/2*(delta_x_count-2.16)/0.2);
            }
            else
            {
                stairs_height = step_height;
            }
            right_leg_position[2] = big_len+small_len+foot_height-delta_z - step_height + step_height*sin(M_PI/2*(delta_x_count-2.16)/0.5);
            left_leg_position[2] = big_len+small_len+foot_height-delta_z-100*sin(M_PI*(delta_x_count-2.16)/0.5);
        }
        Inverse(left_leg_position, right_leg_position, joints, true);
        if(delta_x_count > 0.54 && delta_x_count <= 0.81)
        {
            joints[10] = -0.84495043 + 0.2 * sin(M_PI*(delta_x_count-0.54)/0.27);
        }
        else if(delta_x_count > 0.81 && delta_x_count <= 1.08)
        {
            joints[10] = -0.84495043 + 0.37645495 * (1-cos(M_PI/2*(delta_x_count-0.81)/0.27));
        }
        else if(delta_x_count > 1.08 && delta_x_count <= 1.35)
        {
            joints[4] = -0.84495043 + 0.2 * sin(M_PI*(delta_x_count-1.08)/0.27);
        }
        else if(delta_x_count > 1.35 && delta_x_count <= 1.62)
        {
            joints[4] = -0.84495043 + 0.37645495 * (1-cos(M_PI/2*(delta_x_count-1.35)/0.27));
        }
        else if(delta_x_count > 1.62 && delta_x_count <= 1.89)
        {
            joints[10] = -0.84495043 + 0.2 * sin(M_PI*(delta_x_count-1.62)/0.27);
        }
        else if(delta_x_count > 1.89 && delta_x_count <= 2.16)
        {
            joints[10] = -0.84495043 + 0.37645495 * (1-cos(M_PI/2*(delta_x_count-1.89)/0.27));
        }
        else if(delta_x_count > 2.16 && delta_x_count <= 2.41)
        {
            joints[4] = -0.84495043 + 0.2 * sin(M_PI*(delta_x_count-2.16)/0.25);
        }
        else if(delta_x_count > 2.41 && delta_x_count <= 2.66)
        {
            joints[4] = -0.84495043 + 0.37645495 * (1-cos(M_PI/2*(delta_x_count-2.41)/0.25));
        }
    }

    return true;
}

void Walk::back(float step_len, float *joints)
{
    if(status == "stop" || status == "standing")
    {
        return;
    }
    delta_x_count += 0.001;
    float amplitude = 45;
    if(status == "ready")
    {
        // 起步阶段
        // 第一小节腿不抬起，重心移至右脚
        if(delta_x_count <= 0.33)
        {
            left_leg_position[0] = amplitude*(1-cos(M_PI/2*(delta_x_count/0.33)));
            right_leg_position[0] = amplitude*(1-cos(M_PI/2*(delta_x_count/0.33)));
        }
        // 第二小节左腿往前迈，腿迈快一点
        if(delta_x_count > 0.33 && delta_x_count <= 0.65)
        {
            left_leg_position[0] = amplitude - amplitude*sin(M_PI/2*((delta_x_count-0.33)/0.32));
            right_leg_position[0] = amplitude - amplitude*sin(M_PI/2*((delta_x_count-0.33)/0.32));
            right_leg_position[1] = step_len/2 * (delta_x_count-0.33)/0.32;
            left_leg_position[2] = big_len+small_len+foot_height-delta_z-(15*cos(M_PI*((delta_x_count-0.33)/0.16-1))+15);
            left_leg_position[1] = -step_len/2 * (delta_x_count-0.33)/0.32;
        }
        // 第三小节重心移动至左脚
        if(delta_x_count > 0.65 && delta_x_count <= 0.98)
        {
            left_leg_position[0] = -amplitude*(1-cos(M_PI/2*((delta_x_count-0.65)/0.33)));
            right_leg_position[0] = -amplitude*(1-cos(M_PI/2*((delta_x_count-0.65)/0.33)));
            left_leg_position[1] = -step_len/2 * (1-(delta_x_count-0.65)/0.33);
            right_leg_position[1] = step_len/2 + step_len/2 * (delta_x_count-0.65)/0.33;
        }
        // 第四小节迈右腿
        if(delta_x_count > 0.98 && delta_x_count <= 1.3)
        {
            left_leg_position[0] = -amplitude + amplitude*sin(M_PI/2*((delta_x_count-0.98)/0.32));
            right_leg_position[0] = -amplitude + amplitude*sin(M_PI/2*((delta_x_count-0.98)/0.32));
            left_leg_position[1] = step_len/2 * (delta_x_count-0.98)/0.33;
            right_leg_position[1] = step_len - 1.5*step_len * (delta_x_count-0.98)/0.32;
            right_leg_position[2] = big_len+small_len+foot_height-delta_z-(15*cos(M_PI*((delta_x_count-0.98)/0.16-1))+15);
        }
        if(delta_x_count >= 1.3)
        {
            status = "dynamic";
            delta_x_count = 0;
        }
    }
    else if(status == "dynamic")
    {
        // 第一小节重心移动至右腿
        if(delta_x_count <= 0.33)
        {
            left_leg_position[0] = amplitude*(1-cos(M_PI/2*((delta_x_count)/0.33)));
            right_leg_position[0] = amplitude*(1-cos(M_PI/2*((delta_x_count)/0.33)));
            left_leg_position[1] = step_len/2 + step_len/2 * (delta_x_count)/0.33;
            right_leg_position[1] = -step_len/2 * (1-(delta_x_count)/0.33);
        }
        // 第二小节左腿往前迈
        if(delta_x_count > 0.33 && delta_x_count <= 0.65)
        {
            left_leg_position[0] = amplitude - amplitude*sin(M_PI/2*((delta_x_count-0.33)/0.32));
            right_leg_position[0] = amplitude - amplitude*sin(M_PI/2*((delta_x_count-0.33)/0.32));
            right_leg_position[1] = step_len/2 * (delta_x_count-0.33)/0.32;
            left_leg_position[2] = big_len+small_len+foot_height-delta_z-(15*cos(M_PI*((delta_x_count-0.33)/0.16-1))+15);
            left_leg_position[1] = step_len - 1.5*step_len * (delta_x_count-0.33)/0.32;
        }
        // 第三小节重心移动至左腿
        if(delta_x_count > 0.65 && delta_x_count <= 0.98)
        {
            left_leg_position[0] = -amplitude*(1-cos(M_PI/2*((delta_x_count-0.65)/0.33)));
            right_leg_position[0] = -amplitude*(1-cos(M_PI/2*((delta_x_count-0.65)/0.33)));
            left_leg_position[1] = -step_len/2 * (1-(delta_x_count-0.65)/0.33);
            right_leg_position[1] = step_len/2 + step_len/2 * (delta_x_count-0.65)/0.33;
        }
        // 第四节右腿往前迈
        if(delta_x_count > 0.98 && delta_x_count <= 1.3)
        {
            left_leg_position[0] = -amplitude + amplitude*sin(M_PI/2*((delta_x_count-0.98)/0.32));
            right_leg_position[0] = -amplitude + amplitude*sin(M_PI/2*((delta_x_count-0.98)/0.32));
            left_leg_position[1] = step_len/2 * (delta_x_count-0.98)/0.32;
            right_leg_position[1] = step_len - 1.5*step_len * (delta_x_count-0.98)/0.32;
        }
        if(delta_x_count >= 1.3)
        {
            delta_x_count = 0;
        }
    }

    Inverse(left_leg_position, right_leg_position, joints, true);
}